# plot_multiple_columns_from_dat_float.py
import matplotlib.pyplot as plt
import matplotlib
matplotlib.rc("font",family='YouYuan')
from datetime import datetime

# 读取.dat文件中的数据
print("Reading data from data.dat...")
with open('data.dat', 'r') as file:
    lines = file.readlines()

# 解析数据并获取列数
print("Parsing data and determining number of columns...")
data = [list(map(float, line.strip().split())) for line in lines]
num_columns = len(data[0])

# 打印列数到控制台
print(f"Number of columns in data.dat: {num_columns}")

# 固定选择为 "1"
choice = "2"

# 根据固定的选择绘制图像
if choice == "1":
    # 创建新的图像
    plt.figure()

    # 绘制所有列的数据
    for column in range(num_columns):
        # 提取当前列的数据
        column_data = [row[column] for row in data]

        # 绘制当前列的数据
        plt.plot(column_data, label=f'Column {column + 1}')

    plt.xlabel('Time')
    plt.ylabel('Data')
    plt.title('All Columns Together')
    plt.legend()
    plt.show()
elif choice == "2":
    # 创建并绘制每一列的图像
    print(f"Plotting {num_columns} columns separately...")

    # 创建新的图像
    plt.figure(figsize=(16,8), dpi=100, facecolor='white') 
    

    for column in range(num_columns):
        # 提取当前列的数据
        column_data = [row[column] for row in data]

        if(column != 4):
            # 创建新的图像
            # plt.figure()
            plt.subplot(2, 2, column+1)
            plt.grid(visible=True, which=u'major', axis=u'x') # 添加网格线
 
        # 创建横坐标序列（现在序号*10）
        x = [i * 10 for i in range(1, len(column_data) + 1)]

        # 绘制当前列的数据
        # plt.plot(x, column_data, label=f'Column {column + 1}')
        plt.plot(x, column_data)


        # if(column == 0):
        #     # 找到最小值和最大值的索引
        #     min_index = column_data.index(min(column_data[30:56]))
        #     max_index = column_data.index(max(column_data[30:56]))
        # else:
        #     # 找到最小值和最大值的索引
        #     min_index = column_data.index(min(column_data[30:80]))
        #     max_index = column_data.index(max(column_data[30:80]))

        # 找到最小值和最大值的索引
        min_index = column_data.index(min(column_data[10:100]))
        max_index = column_data.index(max(column_data[10:100]))

        # 在图中绘制最小值和最大值的点
        plt.scatter(x[min_index], column_data[min_index], color='red', label=f'Min {column + 1}')
        plt.scatter(x[max_index], column_data[max_index], color='green', label=f'Max {column + 1}')
        # 在最小值和最大值的点旁边显示坐标
        plt.text(x[min_index], column_data[min_index], f'Min ({x[min_index]}, {column_data[min_index]:.2f})', fontsize=14, ha='left')
        plt.text(x[max_index], column_data[max_index], f'Max ({x[max_index]}, {column_data[max_index]:.2f})', fontsize=14, ha='left')



        plt.xlabel('世界时间 ( ms )')

        count_above_02 = 0

        if(column == 0):
            plt.ylabel('电机力矩 ( n.m )')

            test1 = sum(1 for value in column_data[10:40] if value < -10)
            print(f"Number of values < -10 in Column {column + 1}: {test1}")
            plt.text(x[min_index], column_data[min_index]+10, f'{test1 * 10} ms', fontsize=14, ha='left')

        elif column == 1:
            plt.ylabel('电机速度 (rpm )')
        elif column == 2:
            plt.ylabel('电机位置 ( deg )')
        elif column == 3:
            plt.ylabel('机器人中心高度 ( m )')
        elif column == 4:
            plt.ylabel('足端高度 ( m )')
            # 统计大于0.2的点的个数
            count_above_02 = sum(1 for value in column_data if value > 0.2)
            print(f"Number of values above 0.2 in Column {column + 1}: {count_above_02}")
            plt.text(x[max_index], 0.15, f'{count_above_02 * 10} ms', fontsize=14, ha='left')
            # 画一条y=0.2的水平线
            plt.axhline(y=0.2, color='gray', linestyle='--', label='y=0.2')
        elif column == 5:
            plt.ylabel('机器人中心速度 ( m/s )')
        else:
            plt.ylabel(f'Data in Column {column + 1}')

    # 显示所有图像
    print("Displaying plots...")

    # test = 0.07 / (count_above_02 / 200)
    # print(f"Displa{test}")

    # 保存图片为.png格式，文件名为当前日期和时间（精确到秒）
    current_datetime = datetime.now().strftime("%Y-%m-%d_%H-%M-%S")
    plt.savefig(f"plot_{current_datetime}.png", dpi=300)
    plt.show()

else:
    print("Invalid choice. Please enter 1 or 2.")
